#define MAVLINK_COMM_NUM_BUFFERS 1
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS

// this code was moved from libraries/GCS_MAVLink to allow compile
// time selection of MAVLink 1.0
//BetterStream	*mavlink_comm_0_port;
//BetterStream	*mavlink_comm_1_port;

//mavlink_system_t mavlink_system = {12,1,0,0};

//#include "Mavlink_compat.h"
//#include "include/mavlink/v1.0/mavlink_types.h"
//#include "include/mavlink/v1.0/common/mavlink.h" 

#include "../AQ_MAVLink/include/mavlink_types.h"
#include "../AQ_MAVLink/include/autoquad/mavlink.h"

//Zmienne
/* MAVLink session control */
static bool     mavbeat = 0;
static uint8_t  aq_mav_type;
static uint8_t  aq_mav_system; 
static uint8_t  aq_mav_component;
static bool     mavlink_request = 0;
static bool     displayVersionDone = 0;
static uint16_t messageCounter;
static bool     mavlink_active;
static bool     isClear;


static int packet_drops = 0;
static int parse_error = 0;

void read_mavlink(){
  mavlink_message_t msg; 
  mavlink_status_t status;

  // grabing data 
  while(Serial.available() > 0) { 
    uint8_t c = Serial.read();

    // trying to grab msg  
    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
       messageCounter = 0; 
       mavlink_active = 1;
       switch(msg.msgid)           /* Handle msg */
    {
        case MAVLINK_MSG_ID_HEARTBEAT:
          {
            mavbeat = 1;
	    aq_mav_system    = msg.sysid;
	    aq_mav_component = msg.compid;
            aq_mav_type      = mavlink_msg_heartbeat_get_type(&msg);

//            m2h_mode = mavlink_msg_heartbeat_get_base_mode(&msg);
//            m2h_nav_mode = mavlink_msg_heartbeat_get_custom_mode(&msg);
//            m2h_sysstat = mavlink_msg_heartbeat_get_system_status(&msg);

//            CheckFlightMode();
//            if (m2h_mode == DISARMED) isArmed=0;
//            if ((m2h_mode >= ARMED) || (m2h_sysstat != 3)) isArmed=1;
//            if ((isArmed) & (!startCurLog)) startCurLog =1;            /* start Current logger on 1st armed status */

            //printDebugInfo();
          }
          break;
    }
    
    
  }


}
}
